diff --git a/arch/powerpc/dts/Makefile b/arch/powerpc/dts/Makefile
index ffd929c..0e234cc 100644
--- a/arch/powerpc/dts/Makefile
+++ b/arch/powerpc/dts/Makefile
@@ -6,6 +6,7 @@
 dtb-$(CONFIG_TARGET_P2041RDB) += p2041rdb.dtb
 dtb-$(CONFIG_TARGET_P3041DS) += p3041ds.dtb
 dtb-$(CONFIG_TARGET_P4080DS) += p4080ds.dtb
+dtb-$(CONFIG_TARGET_P5040DS) += p5040ds.dtb
 dtb-$(CONFIG_TARGET_T1024RDB) += t1024rdb.dtb
 dtb-$(CONFIG_TARGET_T1042D4RDB) += t1042d4rdb.dtb
 dtb-$(CONFIG_TARGET_T2080QDS) += t2080qds.dtb
diff --git a/arch/powerpc/dts/p5040.dtsi b/arch/powerpc/dts/p5040.dtsi
new file mode 100644
index 0000000..b6f6c5d
--- /dev/null
+++ b/arch/powerpc/dts/p5040.dtsi
@@ -0,0 +1,62 @@
+// SPDX-License-Identifier: GPL-2.0+ OR X11
+/*
+ * P5040 Silicon/SoC Device Tree Source (pre include)
+ *
+ * Copyright 2012 - 2015 Freescale Semiconductor Inc.
+ * Copyright 2019 NXP
+ */
+
+/dts-v1/;
+
+/include/ "e5500_power_isa.dtsi"
+
+/ {
+	#address-cells = <2>;
+	#size-cells = <2>;
+	interrupt-parent = <&mpic>;
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu0: PowerPC,e5500@0 {
+			device_type = "cpu";
+			reg = <0>;
+			fsl,portid-mapping = <0x80000000>;
+		};
+		cpu1: PowerPC,e5500@1 {
+			device_type = "cpu";
+			reg = <1>;
+			fsl,portid-mapping = <0x40000000>;
+		};
+		cpu2: PowerPC,e5500@2 {
+			device_type = "cpu";
+			reg = <2>;
+			fsl,portid-mapping = <0x20000000>;
+		};
+		cpu3: PowerPC,e5500@3 {
+			device_type = "cpu";
+			reg = <3>;
+			fsl,portid-mapping = <0x10000000>;
+		};
+	};
+
+	soc: soc@ffe000000 {
+		ranges = <0x00000000 0xf 0xfe000000 0x1000000>;
+		reg = <0xf 0xfe000000 0 0x00001000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		device_type = "soc";
+		compatible = "simple-bus";
+
+		mpic: pic@40000 {
+			interrupt-controller;
+			#address-cells = <0>;
+			#interrupt-cells = <4>;
+			reg = <0x40000 0x40000>;
+			compatible = "fsl,mpic", "chrp,open-pic";
+			device_type = "open-pic";
+			clock-frequency = <0x0>;
+		};
+	};
+};
diff --git a/arch/powerpc/dts/p5040ds.dts b/arch/powerpc/dts/p5040ds.dts
new file mode 100644
index 0000000..723d31d
--- /dev/null
+++ b/arch/powerpc/dts/p5040ds.dts
@@ -0,0 +1,18 @@
+// SPDX-License-Identifier: GPL-2.0+ OR X11
+/*
+ * P5040DS Device Tree Source
+ *
+ * Copyright 2012 - 2015 Freescale Semiconductor Inc.
+ * Copyright 2019 NXP
+ */
+
+/include/ "p5040.dtsi"
+
+/ {
+	model = "fsl,P5040DS";
+	compatible = "fsl,P5040DS";
+	#address-cells = <2>;
+	#size-cells = <2>;
+	interrupt-parent = <&mpic>;
+
+};
diff --git a/configs/P5040DS_NAND_defconfig b/configs/P5040DS_NAND_defconfig
index 5368f40..9c9d36e 100644
--- a/configs/P5040DS_NAND_defconfig
+++ b/configs/P5040DS_NAND_defconfig
@@ -2,6 +2,7 @@
 CONFIG_SYS_TEXT_BASE=0xFFF40000
 CONFIG_MPC85xx=y
 CONFIG_TARGET_P5040DS=y
+CONFIG_MPC85XX_HAVE_RESET_VECTOR=y
 CONFIG_FIT=y
 CONFIG_FIT_VERBOSE=y
 CONFIG_OF_BOARD_SETUP=y
@@ -24,6 +25,8 @@
 CONFIG_MP=y
 CONFIG_CMD_EXT2=y
 CONFIG_CMD_FAT=y
+CONFIG_OF_CONTROL=y
+CONFIG_DEFAULT_DEVICE_TREE="p5040ds"
 CONFIG_ENV_IS_IN_NAND=y
 CONFIG_FSL_CAAM=y
 CONFIG_FSL_ESDHC=y
@@ -47,4 +50,3 @@
 CONFIG_FSL_ESPI=y
 CONFIG_USB=y
 CONFIG_USB_STORAGE=y
-CONFIG_OF_LIBFDT=y
diff --git a/configs/P5040DS_SDCARD_defconfig b/configs/P5040DS_SDCARD_defconfig
index f1aa187..e40e055 100644
--- a/configs/P5040DS_SDCARD_defconfig
+++ b/configs/P5040DS_SDCARD_defconfig
@@ -2,6 +2,7 @@
 CONFIG_SYS_TEXT_BASE=0xFFF40000
 CONFIG_MPC85xx=y
 CONFIG_TARGET_P5040DS=y
+CONFIG_MPC85XX_HAVE_RESET_VECTOR=y
 CONFIG_FIT=y
 CONFIG_FIT_VERBOSE=y
 CONFIG_OF_BOARD_SETUP=y
@@ -24,6 +25,8 @@
 CONFIG_MP=y
 CONFIG_CMD_EXT2=y
 CONFIG_CMD_FAT=y
+CONFIG_OF_CONTROL=y
+CONFIG_DEFAULT_DEVICE_TREE="p5040ds"
 CONFIG_ENV_IS_IN_MMC=y
 CONFIG_FSL_CAAM=y
 CONFIG_FSL_ESDHC=y
@@ -46,4 +49,3 @@
 CONFIG_FSL_ESPI=y
 CONFIG_USB=y
 CONFIG_USB_STORAGE=y
-CONFIG_OF_LIBFDT=y
diff --git a/configs/P5040DS_SPIFLASH_defconfig b/configs/P5040DS_SPIFLASH_defconfig
index 7f31813..2b009d2 100644
--- a/configs/P5040DS_SPIFLASH_defconfig
+++ b/configs/P5040DS_SPIFLASH_defconfig
@@ -2,6 +2,7 @@
 CONFIG_SYS_TEXT_BASE=0xFFF40000
 CONFIG_MPC85xx=y
 CONFIG_TARGET_P5040DS=y
+CONFIG_MPC85XX_HAVE_RESET_VECTOR=y
 CONFIG_FIT=y
 CONFIG_FIT_VERBOSE=y
 CONFIG_OF_BOARD_SETUP=y
@@ -24,6 +25,8 @@
 CONFIG_MP=y
 CONFIG_CMD_EXT2=y
 CONFIG_CMD_FAT=y
+CONFIG_OF_CONTROL=y
+CONFIG_DEFAULT_DEVICE_TREE="p5040ds"
 CONFIG_ENV_IS_IN_SPI_FLASH=y
 CONFIG_FSL_CAAM=y
 CONFIG_FSL_ESDHC=y
@@ -46,4 +49,3 @@
 CONFIG_FSL_ESPI=y
 CONFIG_USB=y
 CONFIG_USB_STORAGE=y
-CONFIG_OF_LIBFDT=y
diff --git a/configs/P5040DS_defconfig b/configs/P5040DS_defconfig
index 40d04e9..ae1c963 100644
--- a/configs/P5040DS_defconfig
+++ b/configs/P5040DS_defconfig
@@ -2,6 +2,7 @@
 CONFIG_SYS_TEXT_BASE=0xEFF40000
 CONFIG_MPC85xx=y
 CONFIG_TARGET_P5040DS=y
+CONFIG_MPC85XX_HAVE_RESET_VECTOR=y
 CONFIG_FIT=y
 CONFIG_FIT_VERBOSE=y
 CONFIG_OF_BOARD_SETUP=y
@@ -23,6 +24,8 @@
 CONFIG_MP=y
 CONFIG_CMD_EXT2=y
 CONFIG_CMD_FAT=y
+CONFIG_OF_CONTROL=y
+CONFIG_DEFAULT_DEVICE_TREE="p5040ds"
 CONFIG_ENV_IS_IN_FLASH=y
 CONFIG_FSL_CAAM=y
 CONFIG_FSL_ESDHC=y
@@ -45,4 +48,3 @@
 CONFIG_FSL_ESPI=y
 CONFIG_USB=y
 CONFIG_USB_STORAGE=y
-CONFIG_OF_LIBFDT=y